#include "pid.h"


#if 1
/**
 * @brief 	全向轮pid专用计算函数,默认采用位置式pid
 * @param	get_v				当前值
 * @param	target_v			目标值
 * @retval	输出值
 */
float pid_t::chassis_pid_calc(float get_v, float target_v)
{
	//更新值
	this-> get[PID_ENUM::NOW] = get_v;
    this-> set[PID_ENUM::NOW] = target_v;
    this-> err[PID_ENUM::NOW] = target_v - get_v;

	//死区控制
	if (this-> deadband != 0 && ABS(this->err[NOW]) < this->deadband)
	{
		//清除积分
        this-> integral = 0.0f;
		return 0;
	}

	//pid计算
	/*	i项	*/
	//变速积分
    this-> integral	+=	(this->err[NOW]) * ABS(this->err[NOW]);
	//积分限幅
	abs_limit(&(this-> integral), this-> IntegralLimit);
    this-> iout	= this->i * this->integral;

	/*	d项	*/
	//不微分先行	(基于测量值变化率)
    this-> dout	=	this-> d * (this-> err[NOW] - this-> err[LAST]); 		//(此处将pid控制周期也算入到参数d里面)

	/*	p项	*/
    this-> pout	=	this-> p * this-> err[NOW];

    
	/*	前馈计算	*/
	float feedforward = 1.0f;
	if(this->f != 0)	//前馈系数不为0才计算前馈
	{
		feedforward = this-> f * (this->set[NOW] - this->set[LAST]);	//前馈项
		//前馈限幅
		abs_limit(&feedforward, this->MaxOutput * 0.2f);		//前馈不超过最大输出的20%
	}
	
	/*	计算输出	*/
    this->pos_out = this->pout + this->iout + this->dout + feedforward;
	//输出限幅
    abs_limit(&(this->pos_out), this->MaxOutput);
	//更新上次输出
    this->last_pos_out = this->pos_out;

	//更新数据
    this->err[LLAST] = this->err[LAST];    this->err[LAST] = this->err[NOW];
    this->get[LLAST] = this->get[LAST];    this->get[LAST] = this->get[NOW];
    this->set[LLAST] = this->set[LAST];    this->set[LAST] = this->set[NOW];

	return this->pos_out;
}
#endif

/**
 * @brief 	pid计算函数
 * @param	get_v				当前值
 * @param	target_v			目标值
 * @retval	输出值
 */
float pid_t::pid_calc(float get_v, float target_v)
{
    //更新值
    this->get[PID_ENUM::NOW] = get_v;
    this->set[PID_ENUM::NOW] = target_v;
    this->err[PID_ENUM::NOW] = target_v - get_v;

#if 0       //暂时不用
    //误差过大，不进行输出
	if (pid->max_err != 0 && ABS(pid->err[NOW]) >  pid->max_err ) {
		return 0;
	}
#endif
    //

    //死区控制
    if (this->deadband != 0 && ABS(this -> err[PID_ENUM::NOW]) < this->deadband)
    {
        //清除积分
        if(this -> deaband_integral_clear_mode == PID_ENUM::DEADBAND_CLEAR_I) {
            this->integral = 0.0f;
        }
        return 0;
    }


    //计算
    /*	位置式	*/
    if(this->pid_mode == PID_ENUM::POSITION_PID)
    {
        //计算积分
        this-> integral += this-> err[PID_ENUM::NOW];
        //积分限幅
        abs_limit(&(this-> integral), this-> IntegralLimit);

        //微分先行选项
        float derivate = (this->d_first_mode == PID_ENUM::D_FIRST_EN)?
                         this->set[PID_ENUM::NOW] - this->set[PID_ENUM::LAST]  :
                         this->err[PID_ENUM::NOW] - this->err[PID_ENUM::LAST] ;

        //计算p、i、d三项的输出
        this->pout = this-> p * this-> err[(int)PID_ENUM::NOW];
        this->iout = this-> i * this-> integral;
        this->dout = this-> d * derivate;

        //计算前馈
        /*	前馈计算	*/
        float feedforward = 0.0f;
        if(this->f != 0)	//前馈系数不为0才计算前馈
        {
            feedforward = this-> f * (this->set[PID_ENUM::NOW] - this->set[PID_ENUM::LAST]);//前馈项
            //前馈限幅
            abs_limit(&feedforward, this->MaxOutput * 0.2f);		//前馈不超过最大输出的20%
        }

        //计算输出
        this->pos_out = this->pout + this->iout + this->dout + feedforward;
        //输出限幅
        abs_limit(&(this->pos_out), this->MaxOutput);
        //更新上次输出
        this->last_pos_out = this->pos_out;
    }

#if 0		//目前不用,注释掉
    /*	增量式	*/
    else if(pid->pid_mode == DELTA_PID) 
    {
		//计算p、i、d三项的输出
        pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
        pid->iout = pid->i * pid->err[NOW];
        pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
        //进行积分限幅和输出限幅，得到输出值
        abs_limit(&(pid->iout), pid->IntegralLimit);
        pid->delta_u = pid->pout + pid->iout + pid->dout;
        pid->delta_out = pid->last_delta_out + pid->delta_u;
        abs_limit(&(pid->delta_out), pid->MaxOutput);
		//更新上次输出
        pid->last_delta_out = pid->delta_out;	
    }
#endif
    //

    //更新数据
    this->err[PID_ENUM:: LLAST] = this->err[PID_ENUM:: LAST];
    this->err[PID_ENUM:: LAST]  = this->err[PID_ENUM:: NOW];
    this->get[PID_ENUM:: LLAST] = this->get[PID_ENUM:: LAST];
    this->get[PID_ENUM:: LAST]  = this->get[PID_ENUM:: NOW];
    this->set[PID_ENUM:: LLAST] = this->set[PID_ENUM:: LAST];
    this->set[PID_ENUM:: LAST]  = this->set[PID_ENUM:: NOW];

    //返回计算值
    return this->pid_mode == PID_ENUM::POSITION_PID ? this->pos_out : this->delta_out;
}


/**
 * @brief 	初始化pid参数
 * @param 	mode				pid模式		@args POSITION_PID （位置式）或 DELTA_PID（增量式）
 * @param	d_first_mode		微分先行模式	@args D_FIRST_EN (使能)	或 D_FIRST_DISEN (禁止)
 * @param 	maxout				输出限幅值
 * @param 	intergral_limit		积分限幅值
 * @param 	kp, ki, kd, kf		pid参数
 */
void pid_t::pid_param_init(
        PID_ENUM 	mode, PID_ENUM 	D_first_mode,
        PID_ENUM deadzone_i_clear_mode,
        float 	maxout, float 	intergral_limit,
        float deadzone, float  kp, float ki, float kd, float kf)
{
    this->pid_mode = mode;
    this->d_first_mode = D_first_mode;
    this->deaband_integral_clear_mode = deadzone_i_clear_mode;
    this->MaxOutput = maxout;
    this->IntegralLimit = intergral_limit;
    this->deadband = deadzone;
    this->p = kp;
    this->i = ki;
    this->d = kd;
    this->f = kf;

}

/**
 * @brief 	判断pid是否调到了稳态
 * @retval  @args   FINISH (到达稳态), UNFINISH (未达到稳态)
 */
pid_t::PID_ENUM pid_t::steady_check()
{
    if(
            ABS(this->err[PID_ENUM:: NOW]) < this-> deadband   &&
            ABS(this->err[PID_ENUM:: LAST]) < this-> deadband  &&
            ABS(this->err[PID_ENUM:: LLAST]) < this-> deadband
            )
    {
        return PID_ENUM:: FINISH ;
    }
    else return PID_ENUM:: UNFINISH ;
}


